#ifndef OOPSMP_UTILS_GENERAL_BBOX_BOUNDING_BOX_3D_H_
#define OOPSMP_UTILS_GENERAL_BBOX_BOUNDING_BOX_3D_H_

#include "OOPSMP/Utils/General/BBox/BVGeneric.h"
#include "OOPSMP/Utils/Geometry/Workspace/Part3D.H"

namespace OOPSMP {

	/*
	 *@brief transforms 3d vector
	 *@param m transformation
	 *@param vold old vector
	 *@param vnew empty array to contain transformed vector
	 */
	static inline void transformVector(const double *m, const double *vold, double *vnew)
	{
		vnew[0] = m[0]*vold[0] + m[1]*vold[1] + m[2] *vold[2] + m[3];
		vnew[1] = m[4]*vold[0] + m[5]*vold[1] + m[6] *vold[2] + m[7];
		vnew[2] = m[8]*vold[0] + m[9]*vold[1] + m[10]*vold[2] + m[11];
	}

	/**
     *@brief Macro for class, class pointer, and const class pointer declarations
     */  
	ForwardClassDeclaration(BoundingBox3D);
	
	/**
    *@brief bounding box 3d
    */
	class BoundingBox3D : public BVGeneric {

	public:
		
		/**
         *@brief Default constructor for bbox
		 */
		BoundingBox3D() : BVGeneric() { }

		/**
         *@brief Returns number of elements in bbox array
		 */
		virtual long getNumElements() { return 14; }

		/**
         *@brief Calculates bbox given vertices
		 *@param m_vertices vertices
		 *@param n_vertices number of vertices
		 */
		void calculateBoundingBox( const double * m_vertices, const long n_vertices ) {

			bbox.clear();
			bbox.resize( getNumElements() );

			bbox[0] = bbox[3] = m_vertices[0];    
			bbox[1] = bbox[4] = m_vertices[1];
			bbox[2] = bbox[5] = m_vertices[2];

			for(int i = 1; i < n_vertices; i++)
			{
					 if(m_vertices[3 * i    ] < bbox[0]) bbox[0] = m_vertices[3 * i];
				else if(m_vertices[3 * i    ] > bbox[3]) bbox[3] = m_vertices[3 * i];

					 if(m_vertices[3 * i + 1] < bbox[1]) bbox[1] = m_vertices[3 * i + 1];
				else if(m_vertices[3 * i + 1] > bbox[4]) bbox[4] = m_vertices[3 * i + 1];

					 if(m_vertices[3 * i + 2] < bbox[2]) bbox[2] = m_vertices[3 * i + 2];
				else if(m_vertices[3 * i + 2] > bbox[5]) bbox[5] = m_vertices[3 * i + 2];
			}

		}

		/**
         *@brief Calculates bbox given vertices
		 *@param m_vertices vertices
		 *@param n_vertices number of vertices
		 *@param tran transformation of vertices
		 *@param out empty array to contain bbox
		 */
		void calculateBoundingBox( const double * m_vertices, const long n_vertices, const double * trans, double * out ) {

			double * transVec = new double[3];

			transformVector( trans, m_vertices, transVec );

			out[0] = out[3] = transVec[0];    
			out[1] = out[4] = transVec[1];
			out[2] = out[5] = transVec[2];

			for(int i = 1; i < n_vertices; i++)
			{

				transformVector( trans, m_vertices + i*3, transVec );

				     if(transVec[0] < out[0]) out[0] = transVec[0];
				else if(transVec[0] > out[3]) out[3] = transVec[0];

				     if(transVec[1] < out[1]) out[1] = transVec[1];
				else if(transVec[1] > out[4]) out[4] = transVec[1];

				     if(transVec[2] < out[2]) out[2] = transVec[2];
				else if(transVec[2] > out[5]) out[5] = transVec[2];
			}

			delete [] transVec;
		}

		/**
         *@brief Calculates bbox for given part
		 *@param part constant pointer to part
		 */
		void calculateBoundingBox( Part_const_t part ) {

			double * out = new double[ getNumElements() ];

			bbox.clear();
			
			calculateBoundingBox( part, out );

			for ( long i = 0 ; i < getNumElements() ; i++ ) {
				bbox.push_back( out[i] );
			}

			bbox.resize( getNumElements() );

			delete [] out;

		}

		/**
         *@brief Calculates bbox for given part
		 *@param part constant pointer to part
		 *@param out empty array to contain bbox
		 */
		void calculateBoundingBox( Part_const_t part, double * out ) {

			const double * vert		= dynamic_cast<Part3D_const_t>(part)->getVertices();
			const int nrVer			= dynamic_cast<Part3D_const_t>(part)->getNrVertices();
			const double * trans	= dynamic_cast<Part3D_const_t>(part)->getTransformation();

			calculateBoundingBox( vert, nrVer, trans, out );

		}
		
		/**
         *@brief Returns dimensions of bbox
		 *@param linkDims empty array to contain dimensions
		 */
		void getBBoxDimensions(double * linkDims) const {
			linkDims[0] = bbox[3] - bbox[0];
			linkDims[1] = bbox[4] - bbox[1];
			linkDims[2] = bbox[5] - bbox[2];
		}

	};
	
	/**
     *@brief Declare factory and specify in source file the functions that should
     *       be registered for plug-and-play functionality
     */ 
	DeclareInstanceFactory(BoundingBox3D, BVGenericFactory);

}

#endif